#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/xfeatures2d.hpp>
#include "sift_api.h"



using namespace std;
using namespace cv;


int main()
{
	//Mat markerEx = imread("/home/wenyi/hbSource/external_marker.png", IMREAD_GRAYSCALE);//external  marker
	//Mat markerIn = imread("/home/wenyi/hbSource/internal_marker.png", IMREAD_GRAYSCALE);//internal  marker
	Mat markerEx = imread("/home/wenyi/hbSource/alternate_external_marker.png", IMREAD_GRAYSCALE);//external  marker
	Mat markerIn = imread("/home/wenyi/hbSource/alternate_internal_marker.png", IMREAD_GRAYSCALE);//internal  marker

	vector<KeyPoint> markerExKp;
	Mat markerExDes;

	bool ret = siftHandleImage(markerEx, markerExKp, markerExDes);

	if(ret == false)
	{
		cout << "siftHandleImage error!"  << endl;
		return -1;
	}
#if 0
	Mat featureShow;
	drawKeypoints(markerEx, markerExKp, featureShow, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    imwrite("./testImage/featureShowEx.jpg",featureShow);
#endif

	vector<KeyPoint> markerInKp;
	Mat markerInDes;

	ret = siftHandleImage(markerIn, markerInKp, markerInDes);

	if(ret == false)
	{
		cout << "siftHandleImage error!"  << endl;
		return -1;
	}
#if 0
	drawKeypoints(markerIn, markerInKp, featureShow, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    imwrite("./testImage/featureShowIn.jpg",featureShow);
#endif

	cv::VideoCapture capture("/home/wenyi/workspace/dji/video/test6.mp4");

	if (!capture.isOpened())
	{
		cout << "video file open error." << endl;
		return -1;
	}

	int width = capture.get(cv::CAP_PROP_FRAME_WIDTH);
	int height = capture.get(cv::CAP_PROP_FRAME_HEIGHT);
	int count = capture.get(cv::CAP_PROP_FRAME_COUNT);
	double fps = capture.get(cv::CAP_PROP_FPS);
	cout << "resolution: " << width << " * " << height << endl;
	cout << "count: " << count << endl;
	cout << "fps: " << fps << endl;
	double interval = 1.0 / fps;
	cout << "interval: " << interval << endl;

	cv::Mat im(cv::Size(width, height), CV_16UC4);
	cv::Mat imGray;

	while(1)
	{
		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

		capture >> im;

		if (nullptr == im.ptr() || 0 == im.rows || 0 == im.cols)
		{
			break;
		}

		cvtColor(im, imGray, CV_BGR2GRAY);


		if(imGray.empty())
		{
			cout << "image empty" << endl;
			break;
		}

		vector<Point> rect;
		double rationAngle;
		
		int externalGoodMatchesMinNum;
		int internalGoodMatchesMinNum;
		int returnGoodMatches = 0;
		int inOrEx;

		inOrEx = AllQrCode;
		externalGoodMatchesMinNum = 25;
		internalGoodMatchesMinNum = 18;

		bool ret = computeLandPosition(imGray, markerExKp, markerExDes, markerInKp, markerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches);

		if(ret){
			vector<Point> lines;
			for(int i=0; i<rect.size() - 1; i++)
			{
				lines.push_back(rect[i]);
			}


			float startX, startY, endX, endY;

			startX = std::min({rect[0].x, rect[1].x, rect[2].x, rect[3].x});
			startY = std::min({rect[0].y, rect[1].y, rect[2].y, rect[3].y});
			endX = std::max({rect[0].x, rect[1].x, rect[2].x, rect[3].x});
			endY = std::max({rect[0].y, rect[1].y, rect[2].y, rect[3].y});

			float width, height;
			width = endX - startX;
			height = endY - startY;

			float ratio = height > width?   height / width : width / height;

#if 0
			if(ExternalQrCode == inOrEx && ratio < 1.07f)
			{
				inOrEx = 2;
			}
			else if(InternalQrCode == inOrEx && ratio > 1.07f)
			{
				inOrEx = 1;
			}
#endif

			printf("siftRun-ration-angle:%f, inOrEx:%d, width:%f, height:%f, height_width_ratio:%f, returnGoodMatches:%d\n",
													rationAngle, inOrEx, width, height, ratio, returnGoodMatches);

			Point center;
			center.x = rect[rect.size()-1].x;
			center.y = rect[rect.size()-1].y;

			vector<vector<Point> > pts;
			pts.push_back(lines);

			//Scalar color format is gbr
			drawContours(im, pts, -1, Scalar(0,0,255), 1, 8);

			line(im, Point(center.x - 10, center.y),Point(center.x + 10, center.y), Scalar(0,0,255));
			line(im, Point(center.x, center.y - 10), Point(center.x, center.y + 10), Scalar(0,0,255));
		}

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

		double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
		cout << "ttrack:" << ttrack * 1e3  << endl;

		imshow("video", im);

		// Wait to load the next frame
		int delay = (interval - ttrack) * 1e3;
		delay = delay <= 0 ? 1 : delay;
		waitKey(delay);
	}
	return 0;
}

